#ifndef POLY_UTIL
#define POLY_UTIL
#include <CGAL/Minkowski_sum_2.h>
#include <CGAL/Polygon_with_holes_2.h>
#include "Polygon_triangulation.h"
#include "planner_typedef.h"

Polygon  poly_2_to_poly(Polygon_2 poly)
	{
		Polygon points = Polygon();
		
		//TODO may need to set the points clockwise or other
		for (Polygon_2::Vertex_const_iterator vit = poly.vertices_begin(); vit!=poly.vertices_end();vit++)
		{
			points.push_back(Point(CGAL::to_double(vit->x()),CGAL::to_double(vit->y()),0));
		}
		return points;
	}

Polygon_2  poly_to_poly_2(Polygon points)
	{
		Polygon_2  poly = Polygon_2();

		foreach(Point p, points)
		{
			poly.push_back(Point_2(p[0],p[1]));
		}
		return poly;
	}

Polygon_2  mink_robot(Polygon robot)
	{
		//Point_2 points[]={Point_2(-0.02,-0.02),Point_2(0.02,-0.02), Point_2(0.02,0.02),Point_2(-0.02,0.02)};
		//triacontagon	
		Point_2 points [] = {Point_2(0.02,0),Point_2(0.0195629520147,0.00415823381636),Point_2(0.0182709091529,0.00813473286152),Point_2(0.0161803398875,0.0117557050458),Point_2(0.0133826121272,0.0148628965095),Point_2(0.01,0.0173205080757),Point_2(0.0061803398875,0.0190211303259),Point_2(0.00209056926535,0.0198904379074),Point_2(-0.00209056926535,0.0198904379074),Point_2(-0.0061803398875,0.0190211303259),Point_2(-0.01,0.0173205080757),Point_2(-0.0133826121272,0.0148628965095),Point_2(-0.0161803398875,0.0117557050458),Point_2(-0.0182709091529,0.00813473286152),Point_2(-0.0195629520147,0.00415823381636),Point_2(-0.02,0),Point_2(-0.0195629520147,-0.00415823381636),Point_2(-0.0182709091529,-0.00813473286152),Point_2(-0.0161803398875,-0.0117557050458),Point_2(-0.0133826121272,-0.0148628965095),Point_2(-0.01,-0.0173205080757),Point_2(-0.0061803398875,-0.0190211303259),Point_2(-0.00209056926535,-0.0198904379074),Point_2(0.00209056926535,-0.0198904379074),Point_2(0.0061803398875,-0.0190211303259),Point_2(0.01,-0.0173205080757),Point_2(0.0133826121272,-0.0148628965095),Point_2(0.0161803398875,-0.0117557050458),Point_2(0.0182709091529,-0.00813473286152),Point_2(0.0195629520147,-0.00415823381636)};
		
		Polygon_2 poly_robot = poly_to_poly_2(robot);
		Polygon_2 buff_poly(points,points+3); 

		Polygon_2 p = minkowski_sum_2 (poly_robot,buff_poly).outer_boundary();
		return p;
	}





#endif